#include "OLED_task.h"
#include "FreeRTOS.h"
#include "task.h"
#include "ti_msp_dl_config.h"  // 根据实际硬件驱动调整

#include "E:\CCSproject\DAC\rtostest\targetConfigs\Hardware\change.h"
#include "E:\CCSproject\DAC\rtostest\targetConfigs\Hardware\pid.h"
#include "E:\CCSproject\DAC\rtostest\targetConfigs\Hardware\uart_task.h"
#include "stdio.h"
#include "oled_hardware_i2c.h"


/* 私有函数和变量（仅本文件可见） */
void show_PID(float kp,float ki,float kd)  //显示pid
{
    uint8_t p[4];  //显示需要从16开始，其中要考虑小数点，每次加8
    uint8_t i[3];
    uint8_t d[3];
     // 处理 kp（假设格式为XX.XX，如12.34）
    int kp_scaled = (int)(fabsf(kp) * 100); // 转为整数：1234
    p[0] = kp_scaled / 1000;       // 千位：1
    p[1] = (kp_scaled / 100) % 10; // 百位：2
    p[2] = (kp_scaled / 10) % 10;  // 十位：3
    p[3] = kp_scaled % 10;         // 个位：4
    OLED_ShowNum(16, 0, p[0], 1, 16);
    OLED_ShowNum(24, 0, p[1], 1, 16);

    OLED_ShowNum(40, 0, p[2], 1, 16);
    OLED_ShowNum(48, 0, p[3], 1, 16);
    // 处理 ki（假设格式为X.XX，如0.12）
    int ki_scaled = (int)(fabsf(ki) * 100); // 12
    i[0] = ki_scaled / 100;        // 百位：0
    i[1] = (ki_scaled / 10) % 10;  // 十位：1
    i[2] = ki_scaled % 10;         // 个位：2
    OLED_ShowNum(16, 2, i[0], 1, 16);

    OLED_ShowNum(32, 2, i[1], 1, 16);
    OLED_ShowNum(40, 2, i[2], 1, 16);
    // 处理 kd（同ki）
    int kd_scaled = (int)(fabsf(kd) * 100);
    d[0] = kd_scaled / 100;
    d[1] = (kd_scaled / 10) % 10;
    d[2] = kd_scaled % 10;

    OLED_ShowNum(16, 4, d[0], 1, 16);

    OLED_ShowNum(32, 4, d[1], 1, 16);
    OLED_ShowNum(40, 4, d[2], 1, 16);
}


uint8_t key_scan(void)
{
    uint8_t state = 0,mode = 0;
    if(DL_GPIO_readPins(KEY_KEY5_PORT , KEY_KEY5_PIN) == 0)
    {
        vTaskDelay(pdMS_TO_TICKS(5));
        while(DL_GPIO_readPins(KEY_KEY5_PORT , KEY_KEY5_PIN) == 0);
        vTaskDelay(pdMS_TO_TICKS(5));
        return  5;
    }
    else {
        state = 0;
    }

    if(DL_GPIO_readPins(KEY_KEY6_PORT , KEY_KEY6_PIN) == 0)
    {
        vTaskDelay(pdMS_TO_TICKS(5));
        while(DL_GPIO_readPins(KEY_KEY6_PORT , KEY_KEY6_PIN) == 0);
        vTaskDelay(pdMS_TO_TICKS(5));
        return  6;
    }
    else {
        state = 0;
    }

    if(DL_GPIO_readPins(KEY_KEY7_PORT , KEY_KEY7_PIN) == 0)
    {
        vTaskDelay(pdMS_TO_TICKS(5));
        while(DL_GPIO_readPins(KEY_KEY7_PORT , KEY_KEY7_PIN) == 0);
        vTaskDelay(pdMS_TO_TICKS(5));
        return  7;
    }
    else {
        state = 0;
    }
    return state;
}
#ifdef MASTER_MODE      
uint8_t getnum(void)   //获取拨码开关的值
{
    uint8_t state = 0,mode = 0;
    if(DL_GPIO_readPins(KEY_KEY1_PORT , KEY_KEY1_PIN) == 0)
    {
        state = 0;
    }
    else {
        state = 1;
    }
    mode += state;
    OLED_ShowNum(120, 6, state, 1, 16);

    if(DL_GPIO_readPins(KEY_KEY2_PORT , KEY_KEY2_PIN) == 0)
    {
        state = 0;
    }
    else {
        state = 1;
    }
    mode += state * 2;
    OLED_ShowNum(112, 6, state, 1, 16);

    if(DL_GPIO_readPins(KEY_KEY3_PORT , KEY_KEY3_PIN) == 0)
    {
        state = 0;
    }
    else {
        state = 1;
    }
    mode += state * 4;
    OLED_ShowNum(104, 6, state, 1, 16);

    if(DL_GPIO_readPins(KEY_KEY4_PORT , KEY_KEY4_PIN) == 0)
    {
        state = 0;
    }
    else {
        state = 1;
    }
    mode += state * 8;
    OLED_ShowNum(96, 6, state, 1, 16);

    OLED_ShowNum(64, 6, mode, 2, 16);
    return mode;
} 
/* 任务主函数 */
void OLEDTask(void *pvParameters)         //主机显示和按键函数
{
    OLED_Init();

    uint32_t cnt=0;
    OLED_ShowString(0, 0, (uint8_t *) "P:", 16);
    OLED_ShowChar(32, 0, (uint8_t) '.', 16);

    OLED_ShowString(0, 2, (uint8_t *) "I:", 16);
    OLED_ShowChar(24, 2, (uint8_t) '.', 16);

    OLED_ShowString(0, 4, (uint8_t *) "D:", 16);
    OLED_ShowChar(24, 4, (uint8_t) '.', 16);

    OLED_ShowString(0, 6, (uint8_t *) "MODE:", 16);

    show_PID(pid_speed.Kp, pid_speed.Ki,pid_speed.Kd);
    /* 任务主循环 */
    uint8_t key_num,mode;
    for (;;) {
        key_num = key_scan();
        if(key_num == 0)
        {
            vTaskDelay(pdMS_TO_TICKS(1));
            continue;
        }
        else if(key_num == 5)   
        {
            mode = getnum();
            switch (mode) {         
                case 14:            //1110        调整p         p+1
                pid_speed.Kp+=1;
                    break;
                case 13:            //1101        p-1
                pid_speed.Kp-=1;
                    break;
                case 12:            //1100        p+0.1
                pid_speed.Kp+=0.1;
                    break;
                case 11:            //1011        p-0.1
                pid_speed.Kp-=0.1;
                    break;
                case 0:
                vTaskDelay(pdMS_TO_TICKS(1000));
                lock = 0;
                    break;




            default:
                    break;
            }
            show_PID(pid_speed.Kp, pid_speed.Ki,pid_speed.Kd);
        }



        vTaskDelay(pdMS_TO_TICKS(1));
    }
    
    /* 理论上不会执行到这里 */
    vTaskDelete(NULL);
}
#else
uint8_t getnum(void)   //获取拨码开关的值
{
    uint8_t state = 0,mode = 0;
    if(DL_GPIO_readPins(KEY_KEY4_PORT , KEY_KEY4_PIN) == 0)
    {
        state = 0;
    }
    else {
        state = 1;
    }
    mode += state;
    OLED_ShowNum(120, 6, state, 1, 16);

    if(DL_GPIO_readPins(KEY_KEY3_PORT , KEY_KEY3_PIN) == 0)
    {
        state = 0;
    }
    else {
        state = 1;
    }
    mode += state * 2;
    OLED_ShowNum(112, 6, state, 1, 16);

    if(DL_GPIO_readPins(KEY_KEY2_PORT , KEY_KEY2_PIN) == 0)
    {
        state = 0;
    }
    else {
        state = 1;
    }
    mode += state * 4;
    OLED_ShowNum(104, 6, state, 1, 16);

    if(DL_GPIO_readPins(KEY_KEY1_PORT , KEY_KEY1_PIN) == 0)
    {
        state = 0;
    }
    else {
        state = 1;
    }
    mode += state * 8;
    OLED_ShowNum(96, 6, state, 1, 16);
    OLED_ShowNum(64, 6, mode, 2, 16);

    return mode;
}
/* 任务主函数 */
void OLEDTask(void *pvParameters)           //从机显示和按键函数
{
    OLED_Init();

    uint32_t cnt=0;
    OLED_ShowString(0, 0, (uint8_t *) "P:", 16);
    OLED_ShowChar(32, 0, (uint8_t) '.', 16);

    OLED_ShowString(0, 2, (uint8_t *) "I:", 16);
    OLED_ShowChar(24, 2, (uint8_t) '.', 16);

    OLED_ShowString(0, 4, (uint8_t *) "D:", 16);
    OLED_ShowChar(24, 4, (uint8_t) '.', 16);

    OLED_ShowString(0, 6, (uint8_t *) "MODE:", 16);
    /* 任务主循环 */
    for (;;) {
        
        show_PID(pid_speed.Kp, pid_speed.Ki,pid_speed.Kd);
        

        vTaskDelay(pdMS_TO_TICKS(200));
    }
    
    /* 理论上不会执行到这里 */
    vTaskDelete(NULL);
}
#endif